#include "BPEBox3d.h"
#include <math.h>



BPEBox3d::BPEBox3d(){
	this->center = BPEPoint3d(0,0,0);
	rotation = BPEVector3d(0,0,0);

	vertices = shared_array<BPEVector3d>(new BPEVector3d[8]);
	for(int i = 0; i<8; i++){
		vertices[i] = BPEVector3d(center,BPEPoint3d(0,0,0));
	}

};
BPEBox3d::BPEBox3d(const BPEPoint3d &center){

	this->center = center;
	rotation = BPEVector3d(0,0,0);

	this->vertices = shared_array<BPEVector3d>(new BPEVector3d[8]);
	for(int i = 0; i<8; i++){
		vertices[i] = BPEVector3d(center,BPEPoint3d(0,0,0));
	}


};

BPEBox3d::BPEBox3d(const BPEPoint3d &center,const shared_array<BPEPoint3d> &vertices){
	
	this->center = center;
	rotation = BPEVector3d(0,0,0);
	this->vertices = shared_array<BPEVector3d>(new BPEVector3d[8]);
	
	for(int i = 0; i<8; i++){
		this->vertices[i] = BPEVector3d(center,vertices[i]);
	}

};
shared_array<plane3d<f32>> BPEBox3d::getPlanes(){
	shared_array<plane3d<f32>> planes = shared_array<plane3d<f32>>(new plane3d<f32>[8]);
	shared_array<BPEVector3d> verticesTemp = this->getVertices();
	vector3df *vertices = new vector3df[8];
	for(int i = 0 ; i < 8; i++)
		vertices[i] = vector3df(verticesTemp[i].getHead().getX(),verticesTemp[i].getHead().getY(),verticesTemp[i].getHead().getZ());



	
	planes[0] = plane3d<f32>(vertices[0],vertices[4],vertices[1]);
	planes[1] = plane3d<f32>(vertices[0],vertices[2],vertices[4]);
	planes[2] = plane3d<f32>(vertices[0],vertices[1],vertices[2]);
	planes[3] = plane3d<f32>(vertices[7],vertices[6],vertices[5]);
	planes[4] = plane3d<f32>(vertices[7],vertices[5],vertices[3]);
	planes[5] = plane3d<f32>(vertices[7],vertices[3],vertices[2]);


	vector3df test = planes[0].Normal;
	test = planes[1].Normal;
	test = planes[2].Normal;
	test = planes[3].Normal;
	test = planes[4].Normal;
	test = planes[5].Normal;



	delete vertices;
	return planes;
	
};
void BPEBox3d::setRotation(BPEVector3d rotation){
//angles calculated in rads
	BPEPoint3d headThis = this->rotation.getHead();
	BPEPoint3d headOther = rotation.getHead();

	BPEPoint3d differences(headOther.getX() - headThis.getX(),headOther.getY() - headThis.getY(),headOther.getZ() - headThis.getZ());
	this->rotation.setHead(headOther);
	for(int i = 0; i<8; i++){
		vertices[i].rotateZY(differences.getX());
		vertices[i].rotateXZ(differences.getY());
		vertices[i].rotateXY(differences.getZ());
	}



};
shared_array<BPEVector3d> BPEBox3d::getVertices(){

	return vertices;

};
BPEPoint3d BPEBox3d::getCenter(){
	return center;
};
void BPEBox3d::setCenter(const BPEPoint3d &c){
	center = c;

	for( int i = 0; i<8; i++)
		vertices[i].moveTo(center);
};
bool BPEBox3d::collide(BPEBox3d *other,BPEVector3d &normal, double &depth){

	
	// seperating axis-plane method

	double minDepth = 1000000;
	vector3df tempNormal(0,0,0);

	BPEPoint3d ownPos = this->getCenter();
	vector3df ownP(ownPos.getX(),ownPos.getY(),ownPos.getZ());
	
	BPEPoint3d otherPos = other->getCenter();
	vector3df otherP(otherPos.getX(),otherPos.getY(),otherPos.getZ());




	shared_array<plane3d<f32>> ownPlanes = this->getPlanes();
	shared_array<BPEVector3d> otherVertices =  other->getVertices();
	bool areApart = true;

	
	for(int i =0 ; i <6; i++){//for each plane

		double maxVertexDepth = 0;
		areApart = true;
		
		EIntersectionRelation3D ownCenter = ownPlanes[i].classifyPointRelation(ownP);
		EIntersectionRelation3D otherCenter = ownPlanes[i].classifyPointRelation(otherP);
		if(ownCenter != otherCenter){	//if their centers have the same relation to the plane we cannot test the collision
		
			for(int j = 0; j < 8; j++){//for each other vertex
				vector3df vertex = vector3df(otherVertices[j].getHead().getX(),otherVertices[j].getHead().getY(),otherVertices[j].getHead().getZ());
				EIntersectionRelation3D otherPoint = ownPlanes[i].classifyPointRelation(vertex);
				if(otherCenter != otherPoint){
					areApart = false;
					//here code to find the depth
					//find curent vertex depth
					double currentDepth = fabs(ownPlanes[i].getDistanceTo(	vertex	) ) ;
					if(currentDepth > maxVertexDepth){
						maxVertexDepth = currentDepth;
					}


				}
			}
			
			if(areApart)//if there is a plane that has the same relation to all the points of the other 3drectangle
				return false;	//there is no collision so return false
			else{
				if(maxVertexDepth < minDepth){
					tempNormal = ownPlanes[i].Normal;
					minDepth = maxVertexDepth;
				}
			}
		}
	}



	shared_array<plane3d<f32>> otherPlanes = other->getPlanes();
	shared_array<BPEVector3d> ownVertices =  this->getVertices();
	for(int i =0 ; i <6; i++){//for each plane

		double maxVertexDepth = 0;
		areApart = true;
		
		EIntersectionRelation3D ownCenter = otherPlanes[i].classifyPointRelation(ownP);
		EIntersectionRelation3D otherCenter = otherPlanes[i].classifyPointRelation(otherP);
		if(ownCenter != otherCenter){	//if their centers have the same relation to the plane we cannot test the collision
		
			for(int j = 0; j < 8; j++){//for each other vertex
				vector3df vertex = vector3df(ownVertices[j].getHead().getX(),ownVertices[j].getHead().getY(),ownVertices[j].getHead().getZ());
				EIntersectionRelation3D ownPoint = otherPlanes[i].classifyPointRelation(vertex);
				if(ownCenter != ownPoint){
					areApart = false;
					//here code to find the depth
					//find curent vertex depth
					double currentDepth = fabs(otherPlanes[i].getDistanceTo(	vertex	) );
					if(currentDepth > maxVertexDepth){
						maxVertexDepth = currentDepth;
					}
					
				}
			}
			
			if(areApart)//if there is a plane that has the same relation to all the points of the other 3drectangle
				return false;	//there is no collision so return false
			else{
				if(maxVertexDepth < minDepth){
					tempNormal = otherPlanes[i].Normal;
					minDepth = maxVertexDepth;
				}
			}
		}
	}

	normal = BPEVector3d(tempNormal.X,tempNormal.Y,tempNormal.Z);
	depth = minDepth;

	//depth is false
	return true;
	

};
